import math
import numpy as np
import copy
import matplotlib.pyplot as plt
import csv
import ast

fig = plt.figure()
ax = plt.axes(projection='3d')
xF = list()
yF = list()
zF = list()
xd = list()
yd = list()
zd = list()
# plt.ion()
time = 0


# 更换轨迹为摆线,xt,yt是终点，init_pos是初始化，init_dotpos是初始化速度
def curve(xt, yt, init_pos, init_dotpos):
    ft = [0, 0, 0]  # x,z
    dft = [0, 0, 0]  # x,z
    xT = xt
    yT = yt
    x0 = init_pos[0]
    y0 = init_pos[1]
    z0 = init_pos[2]
    dx0 = init_dotpos[0]
    dy0 = init_dotpos[1]
    Tf = 0.3
    t = time / 0.3
    zh = 0.1
    phasePI = 2 * np.pi * t
    ft[0] = (xT - x0) * (phasePI - np.sin(phasePI)) / (2 * np.pi) + x0
    ft[1] = (yT - y0) * (phasePI - np.sin(phasePI)) / (2 * np.pi) + y0
    ft[2] = zh * (1 - np.cos(phasePI)) / 2 + z0
    dft[0] = (xT - x0) * (1 - np.cos(phasePI)) / Tf
    dft[1] = (yT - y0) * (1 - np.cos(phasePI)) / Tf
    dft[2] = zh * np.pi * np.sin(phasePI) / Tf
    return [ft, dft]


while time<= 0.3:
    time += 0.001
    x, y = curve(2, 10, [0, 0, 0], [0, 0, 0])
    xF.append(x[0])
    yF.append(x[1])
    zF.append(x[2])
    xd.append(y[0])
    yd.append(y[1])
    zd.append(y[2])

    # print(x)
ax.plot3D(xF, yF, zF, c="blue")
# ax.plot3D(xd, yd, zd, c="green")

plt.show()
